//-----------------------------------------------------------------------
//  Microsoft Robotics Developer Studio CodePlex Samples
//
//  Copyright (C) 2010 Microsoft Corporation.  All rights reserved.
//
//  This code released under the terms of the Microsoft Public License
//  (MS-PL, http://opensource.org/licenses/ms-pl.html)
//
//  $File: ThirdSumoPlayer.cs $ $Revision: 1 $
//-----------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;
using W3C.Soap;
using System.IO;
using Microsoft.Robotics.PhysicalModel;
using System.Reflection;
using bumper = Microsoft.Robotics.Services.ContactSensor.Proxy;
using cons = Microsoft.Dss.Services.Constructor;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using physicalmodelproxy = Microsoft.Robotics.PhysicalModel.Proxy;
using webcam = Microsoft.Robotics.Services.WebCam.Proxy;
using create = Microsoft.Robotics.Services.IRobot.Create.Proxy;
using irobot = Microsoft.Robotics.Services.IRobot.Roomba.Proxy;
using sensorupdates = Microsoft.Robotics.Services.IRobot.SensorUpdates.Proxy;
using irobotlite = Microsoft.Robotics.Services.IRobot.Lite.Proxy;
using Microsoft.Ccr.Core.Arbiters;

namespace Microsoft.Robotics.Services.ThirdThirdSumoPlayer
{

    /// <summary>
    /// Implementation class for ThirdSumoPlayer
    /// </summary>
    [DisplayName("ThirdSumoPlayer Service")]
    [Description("An implementation of a Sumo player service.")]
    [DssCategory("http://schemas.microsoft.com/robotics/2007/02/sumo/categories/sumoplayer.html")]
    [Contract(Contract.Identifier)]
    [ActivationSettings(ExecutionUnitsPerDispatcher = 1, ShareDispatcher = false)]
    [DssServiceDescription("http://mrdssamples.codeplex.com")]
    public class ThirdSumoPlayerService : DsspServiceBase
    {
        /// <summary>
        /// This is the name that will show up in the sumo referee
        /// </summary>
        private const string _name = "ThirdThirdSumoPlayer";

        private Port<EmptyValue> _waitForInitializationPort = new Port<EmptyValue>();
        private int _timerCount;


        /// <summary>
        /// _state
        /// </summary>
        [InitialStatePartner(Optional = true, ServiceUri = ServicePaths.Store + "/iRobot.ThirdSumoPlayer.config.xml")]
        private ThirdSumoPlayerState _state = new ThirdSumoPlayerState();

        /// <summary>
        /// _main Port
        /// </summary>
        [ServicePort("/sumoplayer", AllowMultipleInstances=true)]
        private ThirdSumoPlayerOperations _mainPort = new ThirdSumoPlayerOperations();

        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public ThirdSumoPlayerService(DsspServiceCreationPort creationPort) :
                base(creationPort)
        {
        }


        // Partner with IRobotLite (either simulated or actual hardware)
        [Partner(Contract.Identifier + ":irobot",
            Contract = irobotlite.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry,
            Optional = true)]
        irobotlite.IRobotLiteOperations _robotPort = new irobotlite.IRobotLiteOperations();

        [Partner("robotcam", Contract = webcam.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry,
            Optional = true)]
        webcam.WebCamOperations _cameraPort = new webcam.WebCamOperations();

        // used to activate delegates after a set time period
        Port<DateTime> _timerPort = new Port<DateTime>();

        Byte[] _imageBits = new Byte[64 * 64 * 4];

        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            // Publish the service to the local Node Directory
            DirectoryInsert();
            InitializeState();

            // connect with the hardware
            SpawnIterator(ConnectWithRobot);
        }

        /// <summary>
        /// Set up a connection with the Robot.
        /// </summary>
        /// <returns></returns>
        IEnumerator<ITask> ConnectWithRobot()
        {
            #region irobot setup
            bool success = true;
            if (_robotPort == null)
            {
                // Start the default iRobotLite service
                ServiceInfoType info = new ServiceInfoType(irobotlite.Contract.Identifier);
                cons.Create create = new cons.Create(info);
                create.TimeSpan = DsspOperation.DefaultShortTimeSpan;

                ConstructorPort.Post(create);
                yield return Arbiter.Choice(
                    create.ResponsePort,
                    delegate(CreateResponse createResponse)
                    {
                        _robotPort = ServiceForwarder<irobotlite.IRobotLiteOperations>(createResponse.Service);
                    },
                    delegate(Fault fault)
                    {
                        success = false;
                        LogError(fault);
                    }
                );
            }

            if (!success)
            {
                this.Shutdown();
                yield break;
            }

            irobot.RoombaState hardwareConfig = null;
            yield return Arbiter.Choice(_robotPort.Get(),
                delegate(irobot.RoombaState getConfig)
                {
                    hardwareConfig = getConfig;
                },
                delegate(Fault fault)
                {
                    LogError(fault);
                    hardwareConfig = null;
                });

            if (hardwareConfig == null)
            {
                this.Shutdown();
                yield break;
            }

            byte[] robotImage = GetEmbeddedImage();
            if (robotImage != null ||
                hardwareConfig.PollingInterval <= 0 ||
                hardwareConfig.Name != _state.Name ||
                !hardwareConfig.WaitForConnect ||
                !hardwareConfig.StopOnResync)
            {
                if (hardwareConfig.PollingInterval <= 0)
                    hardwareConfig.PollingInterval = 100;

                if (hardwareConfig.Name != _state.Name)
                    hardwareConfig.Name = _state.Name;

                hardwareConfig.WaitForConnect = true;
                hardwareConfig.StopOnResync = true;
                hardwareConfig.RobotImage = robotImage;

                yield return Arbiter.Choice(_robotPort.Configure(hardwareConfig),
                    delegate(DefaultReplaceResponseType ok) { },
                    delegate(Fault fault)
                    {
                        LogError("Failed to update the Robot Image");
                    });
            }

            success = false;

            while (!success)
            {
                // Wait for the connection to complete
                yield return Arbiter.Choice(_robotPort.Connect(),
                    delegate(DefaultUpdateResponseType ok)
                    {
                        LogInfo("Connected to the iRobot Create!");
                        success = true;
                    },
                    delegate(Fault fault)
                    {
                        LogError(fault);
                    });

                if (!success)
                {
                    yield return Arbiter.Receive(false, TimeoutPort(1000), delegate(DateTime wait) { });
                    LogInfo(LogGroups.Console, "Retry: Initializing the iRobot Create!");
                }
            }
            #endregion

            if (_cameraPort == null)
            {
                LogInfo(LogGroups.Console, "ThirdSumoPlayer is operating without the camera.");
            }

            ActivateDsspOperationHandlers();

            MainPortInterleave.CombineWith(
                Arbiter.Interleave(
                    new TeardownReceiverGroup(),
                    new ExclusiveReceiverGroup
                    (
                        Arbiter.ReceiveWithIterator(true, _timerPort, TimerHandler),
                        Arbiter.ReceiveWithIterator(true, _waitForInitializationPort, WaitForInitializationHandler)
                    ),
                    new ConcurrentReceiverGroup()
                )
            );

            Activate(Arbiter.Receive(false, TimeoutPort((int)_state.PollingFrequency / 4), _timerPort.Post));

            yield break;
        }

        /// <summary>
        /// Initialize and Validate starting state.
        /// </summary>
        private void InitializeState()
        {
            if (_state == null)
                _state = new ThirdSumoPlayerState();

            if (_state.PollingFrequency <= 0.0)
                _state.PollingFrequency = 50.0;

            if (_state.Sensors == null)
                _state.Sensors = new Sensors();

            if (_state.LineThreshHoldHigh == 0 && _state.LineThreshHoldLow == 0)
            {
                _state.LineThreshHoldLow = 600;
                _state.LineThreshHoldHigh = 5001;
            }

            if (string.IsNullOrEmpty(_state.Name))
                _state.Name = _name;

            _state.NextModeCheck = DateTime.MinValue;
            _state.LastImage = DateTime.MinValue;
            _state.LastIRobotSensors = DateTime.MinValue;
            _state.SumoMode = SumoMode.Uninitialized;

            SaveState(_state);
        }


        /// <summary>
        /// Wait until the iRobot service is initialized
        /// Then place the Sumo Player in Ready state.
        /// </summary>
        /// <param name="empty"></param>
        /// <returns></returns>
        IEnumerator<ITask> WaitForInitializationHandler(EmptyValue empty)
        {
            _state.SumoMode = SumoMode.Initializing;
            bool success = false;

            for (int ix = 0; !success && ix < 5; ix++)
            {
                // Set iRobot to Full mode and maintain this setting.
                yield return Arbiter.Choice(
                    _robotPort.RoombaSetMode(irobot.RoombaMode.Full, true),
                        delegate(irobot.RoombaCommandReceived ok)
                        {
                            success = true;
                        },
                        DefaultFaultHandler
                );
            }

            if (success)
            {
                // Make sure we are stopped
                _state.RightSpeed = 0;
                _state.LeftSpeed = 0;
                _robotPort.CreateDriveDirect(_state.RightSpeed, _state.LeftSpeed);

                // Set Advanced LED On and Power LED to bright Red.
                _robotPort.RoombaSetLeds(irobot.RoombaLedBits.CreateAdvance, 100, 255);

                // Update the state to Ready
                _state.SumoMode = SumoMode.Ready;
                LogVerbose(LogGroups.Console, "Sumo Mode: Ready");

                // Play the ready tune.
                _robotPort.RoombaPlaySong(2);

            }
        }

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }


        /// <summary>
        /// Custom Drop Handler
        /// </summary>
        /// <param name="drop"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Teardown)]
        public virtual IEnumerator<ITask> DropHandler(DsspDefaultDrop drop)
        {
            if (_state.SumoMode > SumoMode.Ready)
                _state.SumoMode = SumoMode.Ready;

            if (_state.LeftSpeed != 0 || _state.RightSpeed != 0)
            {
                yield return Arbiter.Choice(
                    _robotPort.CreateDriveDirect(0, 0),
                        delegate(irobot.RoombaCommandReceived ok) { },
                        delegate(Fault fault) { });

                _state.LeftSpeed = 0;
                _state.RightSpeed = 0;
            }

            if (_cameraPort != null)
            {
                _cameraPort.DsspDefaultDrop();
                _cameraPort = null;
            }

            if (_robotPort != null)
            {
                _robotPort.DsspDefaultDrop();
                _robotPort = null;
            }

            base.DefaultDropHandler(drop);
        }

        /// <summary>
        /// Process the Robot Sensors
        /// </summary>
        /// <param name="createState"></param>
        private void IRobotStateHandler(irobot.RoombaState createState)
        {
            if ((_state.SumoMode == SumoMode.Uninitialized) &&
                ((createState.Mode == irobot.RoombaMode.Passive) ||
                (createState.Mode == irobot.RoombaMode.Safe) ||
                (createState.Mode == irobot.RoombaMode.Full)
                ))
            {
                _state.SumoMode = SumoMode.Initializing;
                _waitForInitializationPort.Post(EmptyValue.SharedInstance);
                return;
            }
            // ********************************************************************************************

            try
            {
                RobotUpdateFloorSensorsHandler(createState.CliffDetail);
                RobotUpdateBumpersHandler(createState.Sensors);
                RobotUpdateButtonsHandler(createState.Pose);
            }
            catch (Exception ex)
            {
                LogError(ex);
            }
        }


        /// <summary>
        /// Watch the floor
        /// </summary>
        /// <param name="cliffDetail"></param>
        public void RobotUpdateFloorSensorsHandler(create.ReturnCliffDetail cliffDetail)
        {
            const double maxLatency = 500.0;

            // Update our view of the sensors
            _state.Sensors.LineLeft = ((cliffDetail.CliffLeftSignal >= _state.LineThreshHoldLow)
                && (cliffDetail.CliffLeftSignal <= _state.LineThreshHoldHigh));

            _state.Sensors.LineFrontLeft = ((cliffDetail.CliffFrontLeftSignal >= _state.LineThreshHoldLow)
                && (cliffDetail.CliffFrontLeftSignal <= _state.LineThreshHoldHigh));

            _state.Sensors.LineRight = ((cliffDetail.CliffRightSignal >= _state.LineThreshHoldLow)
                && (cliffDetail.CliffRightSignal <= _state.LineThreshHoldHigh));

            _state.Sensors.LineFrontRight = ((cliffDetail.CliffFrontRightSignal >= _state.LineThreshHoldLow)
                && (cliffDetail.CliffFrontRightSignal <= _state.LineThreshHoldHigh));

            _state.Sensors.LineDetected = (_state.Sensors.LineFrontLeft || _state.Sensors.LineFrontRight || _state.Sensors.LineLeft || _state.Sensors.LineRight);

            // Check the latency
            DateTime now = DateTime.Now;
            double iRobotSensorLatencyMs = now.Subtract(cliffDetail.Timestamp).TotalMilliseconds;
            if (_state.LastIRobotSensors != cliffDetail.Timestamp)
            {
                // always update the LastIRobotSensors state
                _state.LastIRobotSensors = cliffDetail.Timestamp;

                // Note: Since this happens above the check for SumoMode.Pending,
                //       it is imperative that we check SumoMode here also.
                if (_state.SumoMode == SumoMode.Blind && iRobotSensorLatencyMs <= maxLatency)
                {
                    LogVerbose(LogGroups.Console, "iRobot sensor latency was high, but is now back to normal!");
                    _state.SumoMode = SumoMode.Wander;
                }
            }

            // Take no action if we are stopped
            if (_state.SumoMode <= SumoMode.Pending)
                return;

            // If sensor latency is high, change mode to SensorsBlind
            if (iRobotSensorLatencyMs > maxLatency)
            {
                // If we are not tracking a recent image
                if (_state.SumoMode != SumoMode.Tracking
                    || now.Subtract(_state.LastImage).TotalMilliseconds > maxLatency)
                {
                    // then we are blind -- take appropriate action!
                    LogVerbose(LogGroups.Console, string.Format("iRobot sensors are {0}ms Old!", iRobotSensorLatencyMs));
                    _state.SumoMode = SumoMode.Blind;
                    // Stop for 1/4 second so that we don't run out of the ring
                    InternalDrivingMilliseconds(0, 0, 250.0);
                    return;
                }
            }

            // If appropriate, take action
            if (_state.SumoMode < SumoMode.AvoidBoundary)
                return;


            if (_state.Sensors.LineDetected)
            {
                _state.SumoMode = SumoMode.AvoidBoundary;
                LogVerbose(LogGroups.Console, "Sumo Mode: AvoidBoundary");
                if (_state.Sensors.LineLeft && !_state.Sensors.LineRight && !_state.Sensors.LineFrontRight)
                    InternalDrivingMilliseconds(-400, -200, 50.0);
                else if (_state.Sensors.LineRight && !_state.Sensors.LineLeft && !_state.Sensors.LineFrontLeft)
                    InternalDrivingMilliseconds(-200, -400, 50.0);
                else
                    InternalDrivingMilliseconds(-200, -200, 50.0);
            }

        }

        /// <summary>
        /// Pose and Buttons Handler
        /// </summary>
        /// <param name="pose"></param>
        /// <returns></returns>
        public void RobotUpdateButtonsHandler(irobot.ReturnPose pose)
        {

            // Ignore buttons before the iRobot is initialized
            if (_state.SumoMode < SumoMode.Ready)
                return;

            if ((pose.ButtonsCreate & create.ButtonsCreate.Play) == create.ButtonsCreate.Play
                && _state.SumoMode < SumoMode.Pending)
            {
                _state.SumoMode = SumoMode.Pending;
                LogVerbose(LogGroups.Console, "Sumo Mode: Pending");

                _robotPort.RoombaPlaySong(3);

                // wait 5 seconds before moving
                InternalDrivingMilliseconds(0, 0, 5000.0);

                // Set Advanced LED On and Power LED to bright Green.
                _robotPort.RoombaSetLeds(irobot.RoombaLedBits.CreatePlay, 0, 255);

            }
            else if ((pose.ButtonsCreate & create.ButtonsCreate.Advance) == create.ButtonsCreate.Advance
                && _state.SumoMode > SumoMode.Ready)
            {
                _state.SumoMode = SumoMode.Ready;
                LogVerbose(LogGroups.Console, "Sumo Mode: Ready");

                // stop driving
                InternalDrivingMilliseconds(0, 0, 1.0);

                _robotPort.RoombaPlaySong(2);

                // Set Advanced LED On and Power LED to bright Red.
                _robotPort.RoombaSetLeds(irobot.RoombaLedBits.CreateAdvance, 255, 255);
            }

        }

        /// <summary>
        /// Receive bumps, cliff, and wall sensors updates
        /// </summary>
        /// <param name="sensors"></param>
        public void RobotUpdateBumpersHandler(irobot.ReturnSensors sensors)
        {
            bool droppedCaster = ((sensors.BumpsWheeldrops & irobot.BumpsWheeldrops.WheelDropCaster) != 0);
            bool droppedLeftWheel = ((sensors.BumpsWheeldrops & irobot.BumpsWheeldrops.WheelDropLeft) != 0);
            bool droppedRightWheel = ((sensors.BumpsWheeldrops & irobot.BumpsWheeldrops.WheelDropRight) != 0);

            bool stallLeft = ((sensors.MotorOvercurrents & irobot.MotorOvercurrents.DriveLeft) != 0);
            bool stallRight = ((sensors.MotorOvercurrents & irobot.MotorOvercurrents.DriveRight) != 0);
            bool stalled = (stallLeft || stallRight);

            bool bumpLeft = ((sensors.BumpsWheeldrops & irobot.BumpsWheeldrops.BumpLeft) != 0);
            bool bumpRight = ((sensors.BumpsWheeldrops & irobot.BumpsWheeldrops.BumpRight) != 0);
            bool bump = bumpLeft || bumpRight;
            bool bumpMode = (_state.SumoMode == SumoMode.Contact);

            // Update Sumo State with the current sensors
            _state.Sensors.LeftWheelStalled = stallLeft;
            _state.Sensors.RightWheelStalled = stallRight;
            _state.Sensors.LeftBumper = bumpLeft;
            _state.Sensors.RightBumper = bumpRight;
            _state.Sensors.DroppedCaster = droppedCaster;
            _state.Sensors.DroppedLeftWheel = droppedLeftWheel;
            _state.Sensors.DroppedRightWheel = droppedRightWheel;
            _state.Sensors.DroppedWheels = droppedCaster || droppedLeftWheel || droppedRightWheel;

            // If Mode supercedes Contact, then we are done here
            if (_state.SumoMode < SumoMode.Contact)
                return;

            SumoMode newMode = _state.SumoMode;
            int left = 500, right = 500;
            double ms = 0.0;

            if (stalled && ((_state.Direction & Direction.Reverse) == Direction.Reverse))
            {
                LogVerbose(LogGroups.Console, "Stalled in Reverse -- Start spinning!");

                left = 500;
                right = -500;
                ms = 250.0;
            }
            else if (bump != bumpMode)
            {
                LogVerbose(LogGroups.Console, "Bumper: " + sensors.BumpsWheeldrops);
                if (bump)
                {
                    if (_state.SumoMode > SumoMode.Contact)
                    {
                        newMode = SumoMode.Contact;
                    }

                    if (bumpLeft && !bumpRight)
                    {
                        left = 250;
                        right = 500;
                        ms = 300.0;
                    }
                    else if (bumpRight && !bumpLeft)
                    {
                        left = 500;
                        right = 250;
                        ms = 300.0;
                    }
                    else
                    {
                        ms = 1500.0;
                    }
                }
                else // no longer in contact
                {
                    // may have overspun, reverse power on left and right wheels
                    left = _state.RightSpeed;
                    right = _state.LeftSpeed;
                    ms = 400.0;

                    newMode = SumoMode.Wander;
                }
            }

            if (ms > 0.0)
            {
                InternalDrivingMilliseconds(left, right, ms);
            }

            if (newMode != _state.SumoMode)
            {
                _state.SumoMode = newMode;
                LogVerbose(LogGroups.Console, "Sumo Mode: " + _state.SumoMode);
            }
        }


        /// <summary>
        ///
        /// </summary>
        /// <param name="cameraFrame"></param>
        private void ValidateFrameHandler(webcam.QueryFrameResponse cameraFrame)
        {
            try
            {
                if (cameraFrame.Frame != null)
                {
                    ImageProcessResult result = null;
                    DateTime begin = DateTime.Now;
                    double msFrame = begin.Subtract(cameraFrame.TimeStamp).TotalMilliseconds;

                    // Ignore old images!
                    if (msFrame < 1000.0)
                    {
                        result = ProcessImage(cameraFrame.Size.Width, cameraFrame.Size.Height, cameraFrame.Frame);
                        double msProcessing = DateTime.Now.Subtract(begin).TotalMilliseconds;
                        LogVerbose(LogGroups.Console, string.Format("{0} Frame {1} ms Processed {2} ms", begin, msFrame, msProcessing));

                        if (result != null)
                        {
                            result.TimeStamp = cameraFrame.TimeStamp;
                            HandleProcessedImage(result);
                        }
                        else
                        {
                            LogVerbose(LogGroups.Console, "Skipping empty camera frame!");
                        }
                    }
                    else
                    {
                        LogVerbose(LogGroups.Console, string.Format("Frame discarded {0} ms old", msFrame));
                    }
                }
            }
            catch (Exception ex)
            {
                LogError(ex);
            }
        }


        /// <summary>
        /// Receive a camera frame from the camera service
        /// </summary>
        /// <param name="result"></param>
        /// <returns>true if the image was processed</returns>
        private bool HandleProcessedImage(ImageProcessResult result)
        {
            bool processImage = false;
            if (result != null)
            {
                _state.ImageResult = result;

                if (_state.SumoMode < SumoMode.Pending)
                    return false;

                if ((_state.SumoMode >= SumoMode.Tracking) &&
                    (result.TimeStamp > _state.LastImage))
                {
                    processImage = true;
                    _state.LastImage = result.TimeStamp;
                }

                if (result.Area > 50)
                {
                    if (processImage)
                        _state.SumoMode = SumoMode.Tracking;

                    if (result.XMean < 0x48)
                    {
                        LogVerbose(LogGroups.Console, "Enemy to the left: " + result.XMean);
                        if (processImage)
                            InternalDrivingMilliseconds(250, 400, 400.0);
                        _robotPort.RoombaSetLeds(irobot.RoombaLedBits.Off, 0, 255);
                    }
                    else if (result.XMean > 0x68)
                    {
                        LogVerbose(LogGroups.Console, "Enemy to the right: " + result.XMean);
                        if (processImage)
                            InternalDrivingMilliseconds(400, 250, 400.0);
                        _robotPort.RoombaSetLeds(irobot.RoombaLedBits.CreateAdvance, 0, 0);
                    }
                    else
                    {
                        LogVerbose(LogGroups.Console, "Enemy Dead Ahead!: " + result.XMean);
                        if (processImage)
                            InternalDrivingMilliseconds(300, 300, 500.0);
                        _robotPort.RoombaSetLeds(irobot.RoombaLedBits.CreatePlay, 0, 0);
                    }
                }
                else
                {
                    // couldn't see the opponent, go back to wander mode
                    if ((processImage) && _state.SumoMode == SumoMode.Tracking)
                    {
                        _state.SumoMode = SumoMode.Wander;
                        _state.NextModeCheck = DateTime.Now;
                        LogVerbose(LogGroups.Console, "Enemy Not Visible");
                    }
                    _robotPort.RoombaSetLeds(irobot.RoombaLedBits.CreatePlay | irobot.RoombaLedBits.CreateAdvance, 0, 255);
                }

            }
            return processImage;
        }

        /// <summary>
        /// Default Error Handler
        /// </summary>
        /// <param name="fault"></param>
        void DefaultFaultHandler(Fault fault)
        {
            LogError(fault);
        }


        /// <summary>
        /// Start driving for a maximum of ms
        /// </summary>
        /// <param name="left"></param>
        /// <param name="right"></param>
        /// <param name="ms"></param>
        public void InternalDrivingMilliseconds(int left, int right, double ms)
        {
            int priorLeft = _state.LeftSpeed;
            int priorRight = _state.RightSpeed;

            if (_state.SumoMode > SumoMode.Pending)
            {
                _state.LeftSpeed = left;
                _state.RightSpeed = right;
            }
            else
            {
                // Shouldn't be moving
                _state.LeftSpeed = 0;
                _state.RightSpeed = 0;
            }

            _state.NextModeCheck = DateTime.Now.AddMilliseconds(ms);

            LogVerbose(LogGroups.Console, string.Format("Driving(L{0}, R{1}, Until {2});", _state.LeftSpeed, _state.RightSpeed, _state.NextModeCheck));

            if (priorLeft != _state.LeftSpeed || priorRight != _state.RightSpeed)
            {
                _state.Direction = Direction.Stopped;
                if (_state.LeftSpeed != 0)
                {
                    _state.Direction |= Direction.Left;
                }
                if (_state.RightSpeed != 0)
                {
                    _state.Direction |= Direction.Right;

                    if (_state.LeftSpeed == _state.RightSpeed)
                    {
                        _state.Direction |= Direction.Straight;
                    }
                    else if (_state.LeftSpeed + _state.RightSpeed == 0)
                    {
                        _state.Direction |= Direction.Spinning;
                    }
                }
                if ((_state.LeftSpeed + _state.RightSpeed) < 0)
                {
                    _state.Direction |= Direction.Reverse;
                }

                // Set a null response port since we don't expect a response
                create.CreateDriveDirect driveDirect = new create.CreateDriveDirect(
                    new create.CmdDriveDirect(_state.RightSpeed, _state.LeftSpeed), null);
                _robotPort.Post(driveDirect);
            }
        }

        /// <summary>
        /// Start driving for count/20's of a second
        /// </summary>
        /// <param name="left"></param>
        /// <param name="right"></param>
        /// <param name="count"></param>
        public void InternalDriving(int left, int right, int count)
        {
            InternalDrivingMilliseconds(left, right, (double)(count * 50));
        }

        /// <summary>
        /// This is the default drive configuration for wander mode
        /// </summary>
        private void SetWanderDrive()
        {
            _state.SumoMode = SumoMode.Wander;
            InternalDrivingMilliseconds(300, -100, 10.0);
        }

        /// <summary>
        /// This function is called periodically.  It evaluates the current
        /// mode and makes a decision about the next action.  If a camera
        /// frame is available and the current mode is appropriate, it is
        /// processed and a decision is made accordingly.
        /// </summary>
        /// <param name="timeout"></param>
        /// <returns></returns>
        public IEnumerator<ITask> TimerHandler(DateTime timeout)
        {
            DateTime now = DateTime.Now;

            _timerCount++;
            try
            {
                if (_state.SumoMode == SumoMode.Initializing)
                {
                    yield break;
                }

                if ((_timerCount % 4) == 0)
                {
                    yield return Arbiter.Choice(
                        _cameraPort.QueryFrame(),
                        ValidateFrameHandler,
                        DefaultFaultHandler
                    );
                }
                else
                {
                    yield return Arbiter.Choice(
                        _robotPort.Get(),
                            IRobotStateHandler,
                            DefaultFaultHandler
                         );

                    if (_state.SumoMode == SumoMode.Initializing)
                    {
                        yield break;
                    }
                }

                if (_state.SumoMode < SumoMode.Pending)
                {
                    if (_state.LeftSpeed != 0 || _state.RightSpeed != 0)
                    {
                        InternalDrivingMilliseconds(0, 0, 1.0);
                    }
                }
                else
                {
                    #region Decide Next Move
                    if (_state.NextModeCheck <= DateTime.Now)
                    {
                        if (_state.SumoMode == SumoMode.Pending)
                        {
                            bool processedImage = false;
                            if (_state.ImageResult != null)
                                processedImage = HandleProcessedImage(_state.ImageResult);

                            if (!processedImage)
                                SetWanderDrive();
                        }
                        else if (_state.SumoMode == SumoMode.Contact)
                        {
                            if (!_state.Sensors.LeftBumper && !_state.Sensors.RightBumper)
                            {
                                // Back to wander mode
                                LogVerbose(LogGroups.Console, "Bumpers Release, New Sumo Mode: Wander");
                                SetWanderDrive();
                            }
                            else if (_state.Sensors.LeftBumper && !_state.Sensors.RightBumper)
                            {
                                LogVerbose(LogGroups.Console, "Still bumped, ahead full!");
                                InternalDrivingMilliseconds(100, -300, 100.0);
                            }
                            else if (!_state.Sensors.LeftBumper && _state.Sensors.RightBumper)
                            {
                                LogVerbose(LogGroups.Console, "Still bumped, ahead full!");
                                InternalDrivingMilliseconds(-300, 100, 100.0);
                            }
                            else
                            {
                                LogVerbose(LogGroups.Console, "Still bumped, ahead full!");
                                InternalDrivingMilliseconds(500, 500, 250.0);
                            }
                        }
                        else if (_state.SumoMode == SumoMode.AvoidBoundary)
                        {
                            // if we just finished avoiding the line
//InternalDrivingMilliseconds(150, 150, 250.0);
                            if (_state.Sensors.LineDetected)
                            {
                                _state.SumoMode = SumoMode.AvoidBoundary;
                                LogVerbose(LogGroups.Console, "Sumo Mode: AvoidBoundary");
                                if (_state.Sensors.LineLeft &&
                                   !_state.Sensors.LineRight &&
                                   !_state.Sensors.LineFrontRight)
                                    InternalDrivingMilliseconds(-100, -400, 200.0);
                                else if (_state.Sensors.LineRight &&
                                        !_state.Sensors.LineLeft &&
                                        !_state.Sensors.LineFrontLeft)
                                    InternalDrivingMilliseconds(-400, -100, 200.0);
                                else
                                    InternalDrivingMilliseconds(-500, -500, 400.0);
                            }
                            _state.SumoMode = SumoMode.Wander;
                        }
                        else if (_state.SumoMode == SumoMode.Tracking)
                        {
                            LogVerbose(LogGroups.Console, "Continue Tracking");
                            if ((_state.LeftSpeed + _state.RightSpeed) == 400)
                                InternalDrivingMilliseconds(200, 200, 250.0);
                            if ((_state.Direction & Direction.Left) == Direction.Left)
                                InternalDrivingMilliseconds(100, 300, 150.0);
                            //else if ((_state.Direction & Direction.Right) == Direction.Left)
                            else if ((_state.Direction & Direction.Right) == Direction.Right)
                                InternalDrivingMilliseconds(300, 100, 150.0);
                            else
                                InternalDrivingMilliseconds(250, 250, 250.0);

                        }
                        else if (_state.SumoMode == SumoMode.Wander)
                        {
                            LogVerbose(LogGroups.Console, "Wander Pattern");
                            SetWanderDrive();
                        }
                        else if (_state.SumoMode == SumoMode.Blind)
                        {
                            // While we are blind spin slowly left & right
                            if ((_state.Direction & Direction.SpinLeft) == Direction.SpinLeft)
                                InternalDrivingMilliseconds(100, -100, 500.0);
                            else
                                InternalDrivingMilliseconds(-100, 100, 500.0);
                        }
                    }
                    #endregion
                }
            }
            finally
            {
                int remain = (int)(_state.PollingFrequency) / 4 - (int)(DateTime.Now.Subtract(now).TotalMilliseconds);
                if (remain < 10)
                {
                    _timerPort.Post(now);
                }
                else
                {
                    Activate(Arbiter.Receive(false, TimeoutPort(remain), _timerPort.Post));
                }
            }

            yield break;
        }

        private ImageProcessResult ProcessImage(int width, int height, byte[] pixels)
        {
            if (pixels == null || width < 1 || height < 1 || pixels.Length < 1)
                return null;

            int offset = 0;
            int threshold = 220 * 3;
#if false
            int[] xProjection = new int[width];
            int[] yProjection = new int[height];
#endif
            int xMean = 0;
            int yMean = 0;
            int area = 0;

            // only process bottom half of the image
            for (int y = height / 2; y < height; y++)
            {
                offset = y * width * 3;
                for (int x = 0; x < width; x++)
                {
                    int r, g, b;

                    b = pixels[offset++];
                    g = pixels[offset++];
                    r = pixels[offset++];

                    if (b + g + r > threshold)
                    {
                        xMean += x;
                        yMean += y;
                        area++;

#if false
                        xProjection[x]++;
                        yProjection[y]++;
#endif
                    }
                }
            }

            if (area > 0)
            {
                xMean = xMean / area;
                yMean = yMean / area;
            }

            ImageProcessResult result = new ImageProcessResult();

            result.XMean = xMean;
            result.YMean = yMean;
            result.Area = area;

#if false
            int xOff = -xMean;
            int xSecond = 0;
            int xThird = 0;

            for (int i = 0; i < xProjection.Length; i++)
            {
                if (xProjection[i] > 0)
                {
                    int square = xOff * xOff * xProjection[i];

                    xSecond += square;
                    xThird += xOff * square;
                }
                xOff++;
            }

            double xStdDev = Math.Sqrt((double)xSecond / area);
            double xSkew = xThird / (area * xStdDev * xStdDev * xStdDev);

            int yOff = -yMean;
            int ySecond = 0;
            int yThird = 0;

            for (int i = 0; i < yProjection.Length; i++)
            {
                if (yProjection[i] > 0)
                {
                    int square = yOff * yOff * yProjection[i];

                    ySecond += square;
                    yThird += yOff * square;
                }
                yOff++;
            }

            double yStdDev = Math.Sqrt((double)ySecond / area);
            double ySkew = yThird / (area * yStdDev * yStdDev * yStdDev);

            result.XStdDev = xStdDev;
            result.YStdDev = yStdDev;
            result.XSkew = xSkew;
            result.YSkew = ySkew;
#endif
            return result;
        }

        /// <summary>
        /// Get the image stored in the embedded file PlayerImage.bmp and return the bits in a Byte array
        /// </summary>
        /// <returns></returns>
        private Byte[] GetEmbeddedImage()
        {
#if !NET_CF
            Stream embeddedStream = GetEmbeddedFile();
            if (embeddedStream == null)
                return _imageBits;   // couldn't find the resource

            using (System.Drawing.Bitmap playerImage = new System.Drawing.Bitmap(embeddedStream))
            {
                if ((playerImage.Width == 64) &&
                    (playerImage.Height == 64) &&
                    (playerImage.PixelFormat == System.Drawing.Imaging.PixelFormat.Format24bppRgb))
                {
                    System.Drawing.Imaging.BitmapData bmd = playerImage.LockBits(
                        new System.Drawing.Rectangle(0, 0, 64, 64),
                        System.Drawing.Imaging.ImageLockMode.ReadOnly,
                        System.Drawing.Imaging.PixelFormat.Format32bppArgb);

                    System.Runtime.InteropServices.Marshal.Copy(bmd.Scan0, _imageBits, 0, 64 * 64 * 4);

                    playerImage.UnlockBits(bmd);
                }
            }
#endif
            return _imageBits;
        }

        /// <summary>
        /// Find the embedded file with the specified name and return a stream to read its contents
        /// </summary>
        /// <returns></returns>
        private Stream GetEmbeddedFile()
        {
            try
            {
                System.Reflection.Assembly asm = System.Reflection.Assembly.GetExecutingAssembly();
                string[] names = asm.GetManifestResourceNames();
                if(names.Length > 0)
                    return asm.GetManifestResourceStream(names[0]);

                return null;
            }
            catch
            {
                return null;
            }
        }
    }
}
